close all; clear; clc;

%% robot

alpha1 = 0; alpha2 = pi/2; alpha3 = 0; alpha4 = pi/2; alpha5 = -pi/2; alpha6 = pi/2;
a2 = 0.18; a3 = 0.58; a4 = 0.16;
d1 = 0.43; d3 = 0.0; d4 = 0.6395; d6 = 0.115;
theta1 = 0; theta2 = pi/2; theta3 = 0; theta4 = 0; theta5 = 0; theta6 = 0;
sigma = 1; q = 0;

Alpha = [alpha1, alpha2, alpha3, alpha4, alpha5, alpha6];
A = [0, a2, a3, a4, 0, 0];
D = [d1, 0, d3, d4, 0, d6];
Theta = [theta1, theta2, theta3, theta4, theta5, theta6];
Sigma = [sigma, sigma, sigma, sigma, sigma, sigma];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;
DH_para.Sigma = Sigma;

n = 10;
Q = rand(n, 6) * pi - pi / 2;

%% base
px = 1; py = 2; pz = 3;
rz = pi/2; ry = pi/3; rx = pi/4;

T_b0 = Translx(px) * Transly(py) * Translz(pz) * Trotz(rz) * Troty(ry) * Trotx(rx);

%% tool
tx = 0.1; ty = 0.2; tz = 0.3;
T_tool = Transl(tx, ty, tz);
